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TECHNICAL MEMORANDUM X-73342 

COMPUTER SIMULATION RESULTS OF AHITUDE ESTIMATION 
OF EARTH ORBITING SATELLITES 

I. INTRODUCTION 


The computer simulation results of attitude estimatimi of Earth-orbiting 
satellites are presented in this report. This is a continuation of previous 
research work published in NASA TM X-64943 HJ . The theory, derivation, 
and applications of decomposed linear recursive filter are described in Refer- 
ence 1 which also shows the practical feasibility and advantages of these filters 
by computer simulation. Six main simulation programs were developed for this 
simulation program, written in BASIC language and executed by cc^.r^uters 
HP 9830A and HP 986GA, and are as follows: 

Program 1 — Gaussian Random Number Generation 

Program II — Inverse of Symmetric Positive Definite Matrices 

Program III — Solutions of Ordinary Differential Equations 

Program IV — Simulation of Dynamic Systems in Stochastic Environ- 
ments 

Program V — Simulation of Attitude Estimation by Kalman Filter 

Program VI — Simulation of Attitude Estimation by Decomposed Linear 
Recursive Filters. 

Program 1 generates Gaussian distributed random numbera with required 
mean value and standard deviation. This program will be used in the modeling 
of stochastic onvironmmital disturbances and sensor measurement noises. The 
formulas of decomposed linear recursive filter involve an inverse of symmetric 
positive definite matrices vdiich can be calculated Program II. Programs 
ni and’IV are used to find solutions of difierential equations and difierence 
equations, respectively. Their results show how wdl the discrete-time model 
(difference equations) represents the corresponding continuous-time model 


r 


T 


t 

I 


(differential equations) throu^ the discretization process. For comparison, 
attitude estimation of Earth-orbiting satellites by Kalman filter has also been 
simulated using Program V. Programs I, II, ni, and IV were used to develop 
the simulation program (Program VI) of attitude estimation by decomposed 
linear recursive filters. These six programs will be described in the following 
sections with programs also shown. 


1 1. GAUSSIAN RANDOM NUMBER GENERATION 

Using the HP 9830 A calculator, uniformly distributed random numbers 
between 0 and 1 can be easily generated by executing RND(x) , where x is a 
dummy argument. Now the problem is how to generate Gaussian (or normally) 
districted random numbers with required mean value and standard deviation. 
According to (2) , an ^proximation to normally distributed random number Y' 
can be found from a sequence of uniformly distributed random numbers using 



where is a uniformly distributed random number between 0 and 1 , and 

N is the to be used. It can be shown that Y* approaches a true normally 

distributed random variable as N approaches infinite. For this program, N i 

was chosen as 12 to reduce execution time; therefore, we have 


12 

Y’ = E X, -6.0 
1=1 


The adjustment for the required mean and standard deviation is then 


Y = Y* * S + M 

where S is the required standard deviation and M is the required mean. 
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Program I-l (Fig. 1) is for the generation of Gaussian distributed 
random numbers by using the previously mentioned method. In Program I-l, 
three parameters should be specified as inputs: M (mean value), S (standard 
deviation), and K (Gaussian random number needed). An example of generat- 
ing Gaussian random numbers with M = 0, S = 1, and K = 30 has been executed 
and listed following the program. 

Program 1-2 ( Fig. 2) is for examining the characteristics of generated 
Gaussian random numbers, i.e. , to calcula<:c; sample mean value and sample 
standard deviation. These values can be u' ed as a measure of whether the 
generated random numbers represent Gaoiisian random numbers satisfactorily. 
An example with mean zero and stai.daru deviation 10~® was executed and the 
results of Program 1-2 gave a sample mean of 1. 132 • 10~^ and a sample 
standard deviation of 1.05 • 10“® for K = 30. These show that the error ir< 
the sample mean value is on the order of 10"'^ and the error in the sample 
standard deviation using 30 samples is less than 5 percent of the required 
standard deviation. 


1 1 1. INVERSE OF SYMMETRIC POSITIVE DEFINITE 

MATRICES 


The BASIC program to find tlie inverse of symmetric |x>sitive definite 
matrices Is presented in this section. The basic idea is first to compute a 
triangular factorization of a symmetric positive definite matrix using the 
square root method of Cholesky [31. This method is formulated in the following 
two parts: 

a) Given an n n symmetric |X>sitivc definite matrix A , w compute 
an upper triangular matrix It such that 


A - It* It 


By Cholesky*s method, the elements r^j^ of R arc computed from the following 
recursive formulas: 
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Ik 


a 


Ik 


r 


11 


k ~~ 1 , 2, . . . , n 
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V^ REM GE.iEPflTlOH OF GHUS?!HN RnMDiifl NUr BER ' ' '■■■ 

20 N=1 " - " 

30 INPUT SjNjF 

35 REM S=SrHNDflRD iiEVIHTIOHj ri"MERt!i k=CflUSSlflH RANDOM NO. 
40 R=0 

50 FOR 1=1 TO 12 
60 B=RND5 
70 fi=R+B 
80 NEXT I 
90 Y=<R-6>^«-3<-M 
100 PRINT Y 
110 N=N+1 

120 IF N<K THEN 40 
130 END 


-1.804205262 
-0.960825294 
0.898244914 
-0.577471438 
1.886124850 
1.2833S8978 
-1.392937854 
-0.059498446 
-0.242939598 
-2.62313211 
-1.105240732 
2.050305586 
0.472294194 
0.489608242 
1.260506930 
1 . 332305458 
0.903455026 
-0.979977166 
0.698572032 
-0.472558330 
-0.341572302 
0.26OS6O466 
0.47OO8747*> 
0.622551922 
0. 128073010 
-0.304474062 
0.77F .^21906 
0.068883114 
0.659347762 


Figure 1. Program I-l; generation of Gaussian random number. 
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NEEDED 



10 REM CHECK rjF GAUSS I AN RANHOM HUMBER 
SO N«1 

30 INPUT S»M>K 

35 REM S«STAHDARD DEV I AT I ON » H='MEAN. K=GAUSSIAN RANDOM 
3S U«0 
37 V«0 
40 A*0 

50 FOR I«1 TO 12 
€0 B*RND5 
70 A*A+B 
80 NEXT I 
90 Y-<A-6)*S-H1 
100 PRINT V 
102 U«U+V 

104 v«V+Yt2 

110 NaN+1 

120 IF N<K THEN 40 

122 U»U/K 

124 V«SQR<V/K> 

126 PRINT U>V 
130 END 


NO. 


NEEDED 


“1.80421E-06 
“9.60025E-07 
8.98245E-07 
-5.77471E-07 
1.88612E-06 
1.28339E>06 
-1.39299E-06 
-5.94984E-08 
-2.42940E-07 
-2.62313E-06 
-1. 10524E-06 
2.05031E-06 
4.72294E-07 
4.89608E-07 
1.26051E-06 
1.33231E<06 
9.03455E-07 
-9.79977E-07 
6.98572E-07 
-4.72558E-07 
-3.41572E-07 
2.6O860E-O7 
4.70087E-07 
6.22352E-07 
1.28073E-07 
.-3.04474E-07 
7.76922E-07 
6.86881E-08 
6.59343E-07 

1.13215E-07 i.05027E-U6 


HIBFPODUCBIUTY of TH75 
AG? 


Figure 2. Progrtm 1-2: obeok of Geueeien random number. 


T 

b) Since (B~*) » to compute A~^ we need to find the inverse 

of tpper triangular matrix R . Now the elements r^^ of R"* are oon^>uted 
from the following jrecursive formula: 


J ** 2| 3f eeef U 

k ® j, j+lf • • • » n 



r., = 0 i > k . 

ik 

T 

After we find R"‘, we can compute A"* = R"‘ (R"*) . 

Program n (Fig. 3) gives the inverse of any 3x3 symmetric positive 
definite matrices. This program can also be used to invert any n x n symmetric 
positive definite matrix by changing the array dimensions of the matrix and the 
statement (with statement number 120). 

In Program II« the input is the A matrix and outputs are R , R**^ and 
A~^ (Note that R'^ is denoted by symbol P and A** is denoted by (symbol 

Q-) 


Example — A 3 x 3 positive definite and symmetric matrix A : 


1 



A = 


1 


1 


2 2 
2 6 


6 



■ ‘ y .••IM Ml ’•« ' i 

li-'O n-^3 

IIIPl'T ii[ 1 . » J *:i J.c- t,Hl M*rH 3-'.J 

: n F:i i» 1 i-S0K‘.iii 1. n> 

1‘jO rOR iJ'.vi TO N 
l.;0 PC l»K3-ftn.K]'MU» j ] 
iro HEXT K 
It-iO J=2 
S=0 

200 Ff:R 1 = 1 lu .1-1 
210 i^-S^ F■r I» .1312 
22(« MfXT i 

2 ;ii F:C J » J 3=Sf'R < fif J • J J -3 > 

2-10 IF J=M THEN 310 
250 FOR K- Ml TO H 
2^0 U=0 

270 FOR ! 1 ro J-1 
200 IJ=U4RU. n-K I*K3 
220 NEXT I 

OC'O K j.». ]=<nt i. ji 

510 Hf.MT I, 

220 J- Ml 

5- ':y OoTO 100 

540 FRIMT PCI . 1 3. PC 1 » 3» Pf I , j J, Rl 2*2 1> !:t 2» 3 3» PC 5* 3 3 

434 DIM PC3.3J 

450 FOP J»1 30 M 

4-.0 pr J, J3=1''RC J»J3 

4*. ;- lltXT J 

4(-4 I-^N-1 

405 FOR K=I+5 70 H 
4 V-0 

470 FOR M=l-*. ro P 

47.-; V--V+Pt MlJPpCtl.r 3 / 

474 HfXT M ' 

4Tv; PC I»KJ=-V. PC I» I 3 
478 MFXT r 
480 I'-I-l 

482 IF I ;y THI.M 4t.5 

404 PPiMr PC li 1 3. PC l»2 3.rc l»33«rr2*2 J*I C2»3 3*P[ 3»5 J 

5 MJ 'in 0C3»5] 

550 FOR 1=2 10 M 
5*'.0 FOR K=1 TO I-l 

570 PC I >K 3=0 — 

500 MFXT K 
500 Mr XT I 

tOO iHp jTl , 1 ., li 
,• jn f I.I.; 1,-1 TO (I 
0 '4 J*L 3--0 
FOP K=l TO M 

►*40 01 J.L J=Ot JfLl-*PC . rj>F(.L»KJ 
C50 MEXT i: 
t...O MEXT L 
570 MEXT J 

680 PRINT OC 1 . 1 3* OC 1 . 2 3» OC 1 * 3 3» OC 2* 2 3. OC 2.5 J. OC 3. 3 3 

6- 0 END 


Figure 3. Program II: inverse of symmetric positive definite matrices. 


7 


is used as iiqiut to execute Program II. The outputs are 



0 0 0.5 


and 


A-‘ = 


-1 


-1 1.25 -0.25 


-0.25 0.25 


The following matrix multiplications are to check the correctness of this pro- 
gram and results. 


AA"‘ = 


1 1 1 
12 2 


2-1 0 
-1 1.25 -0.25 


10 0 
0 1 0 


L 

1 

2 

«J 

1 

0 

-0. 

25, 1 

0.25 

J 

L» 

0 


1 

1 

1 


1 

-1 

0 


’ 1 

0 

o' 

RR"‘ = 

0 

1 

1 


0 

1 

-0.5 

= 

0 

1 

0 


_0 

0 

2 J 


0 

0 

0.5_ 


0 

0 

1_ 


1 


V 
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IV. SOLUTIONS OF ORDINARY DIFFERENTIAL EQUATIONS 


Given a set of first-order ordinary differential equations (Note: high- 
order differential equations can be transformed into a set of first-order differen- 
tial equations) 


dx 

■i ■ i (i.‘) 


with initial value x(ifl) = where x(t) is an n-dimensional vector and f^ is 
a vector- valued function of x and t . The modified Runge-Kutta method by 
Gill 1 3] is used to find the solution to this set of first order differential equa- 
tions. This method improves the storage requirements and accumulated 
roundoff errors of the classical Runge-Kutta method. Now the resulting vector 

x(ifl + h) = X 4 is computed by the following formulas with x(lfl) = Xq and 
Qo = 0: 


k, = h • f (2^, to) 

B = 2«o + I (iSi - 2 Qo) 

-22d)] 

h = h • f to + I) 


- So 2 “ (ki 
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X 2 = a + (i- )ih-9x) 

= h • f to+l^ 

23 = Xz+ (l + ^)(jS3-S2) 

Ss = S 2 + 3 ^^l + ^^(k3-S2)j - + 

isi = ^ * i (b* <« + *') 

24 = 23 + |(1Si-2 23) 

S4 = 23 + 3^^(k4-2Q3)j-^k|, 

Here §[4 represents approximately three times the roundoff error in 3C4 accu- 
mulated during one iteration. It can be used as a check of calculation accuracy. 
For the next iteration* ^ is used as ^ , to -t' h as to * and 24 2^ . 

The adjustment of the stqp size h is done by comparison of the results 
due to double and single st^ size 2h and h . 

Program III (Fig. 4 ) is presented using the modified Bunge-Kutta 
method to solve a fifth-order system: 
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ao 0IMX(6|.QC$I.K(S] 

40 I NFijT KC I ] j ^ i . ;<c 5 3 !« ;-;c 4 j , : ;t 5 ] 

50 T*0 

60 FOR 1=1 TO 5 
70 O.CI]=0 
80 NEXT I 
90 H=100 
95 M*0 
100 0=0 

110 FOR 1=1 TO 12 
120 E:=RNri5 
130 fl=fl*B 
140 NEXT I 

150 3*<fl-6)*0. 00000251 
200 L»1 

210 Km«H*KC2] 

220 KC23-H#<Xt 3]+Xt4]+S) 

230 KC33«0 

240 Kt4I=H*0.0O2»Xt5J 
250 Kt5]=H*(-0.002)«XC4] 

260 IF L>1.1 THEN 345 

270 FOR 1=1 TO 5 

280 XCI]«XCI]+0.5» m-2*Q[I3) 

290 NEXT I 
300 FOR J=1 TO 5 

310 Ot J3=QU]+3*0.5 K£J3-2*C!tJ3)-0.5*KtJ] 

320 NEXT J 
330 L=L+1 
340 GOTO 210 

345 IF L>2.1 THEN 425 . 

350 FOR 1=1 TO 5 

360 xtn=xm+<i-SQR<0.5))*:Kf n-Qcn) 

370 NEXT I 
380 FOR J»1 TO 5 

390 Q[J]=Qt J]+3*'1-SQR<0.5))*<KC J3-QC J3)-<1-SQR<0.5))»KCJ3 • 

400 NEXT J 

410 L-L+1 

420 GOTO 210 

425 IF L>3.1 THEN 510 

430 FOR I-l TO 5 

440 xm=xc n+ < 1 tsoR < 0 . 5 > ) * < Kt n-ot I ) ) 

450 NEXT I 
460 F0RJ*1 TO 5 

470 Q[ Jl=Qt .J3+3*<l-*-SORC0.5> >«<Kr J3-0C J])-<l*SQR<0.5)>*Kt JJ 

480 NEXT J 

490 L=L+1 

500 GOTO 210 

510 FOR 1=1 TO 5 

520 xn]»x[n+(Km-2*om>/t. 

530 NEXT I . I 

540 FOR J=1 TO 5 

550 QtJ3«0U]+;!*<Kt J J-2*0C J])/6-0.5»»:r j; 

560 NEXT J 

570 PRINT T+H»XC 1 ].XI2].Xt33.Xt4I*XC53*G£ 1 3» Q£ 2 3* G(£ 3 3» Ql 4 ]*0£ 5 3 
STS W-0 
MO T-T4H 

SSO JFTOOOtTHEN 100 
too END 


Figure 4. Program III: modified Runge-Kutta method. 


REPRODUCBn-ITY OP Tttl5 

,ix. r \ fg 



dx 

i = 3- = i(x.t) 


= A X + G 8 


0 

1 

0 

0 

0 


0 

0 

0 

1 

1 

0 


1 

0 

0 

0 

0 

0 

X + 

0 

0 

0 

0 

0 

0.002 


0 

_0 

0 

0 

-0.002 

0 


_ 0 _ 


where s is a random variable having Gaussian distribution with mean 0 and 
standard deviation 2. 51 • 10“® , It was found that the results are satisfactory 
for h = 0.01 second. In Program III, the input is the initial value x(0) and the 
ou^uts are time t , x(t) , and ^ . 

One of the purposes of solving this differential equation is that its 
results can be used as a measure of the accuracy of discrete-time model 
thxou^ discretization process which will be described in detail in the next 
section. 


V. SIMULATION OF DYNAMIC SYSTEMS IN 
STOCHASTI C ENVI RONMENTS 


At first we introduce the discretization process (4] in which differential 
equations are transformed into difference equations. Let us rewrite the fifth- 
order system considered in Section III as follows: 


X * A X + G s 
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It can be shown that the solution x(t) of this e<iuation in terms of Initial 
value x(^) iiq>ut s can be given as 


x(t) = e^‘x(0) + / G b(z) dz 


or equivalently 


x(t*At) - e*^‘x(t) * G dz • .(t) 

t 


or using discrete-time system notations, we have 


x(k+l) = 0 x(^) B s(k) 


where x(k) denotes the value of state x at the (k + l)th sampling instant and 
matrices ^ and B are 


1 At 0.5 • (At)^ 250 000 ( 1-cos 0.002 At) 250 000 (0.002 At - sin 0.002 At) 


0 1 At 


0 0 


500 sin 0.002 At 


500 (l-cos 0.002 At) 


0 0 0 


cos 0. 002 At 


sin 0. 002 At 


•sin 0. 002 At 


cos 0. 002 At 


1 



B = 


0.5 • (At)* 
At 
0 
0 
0 


Since s is a Gaussian random input. Equation ( 1) is a discrete>*time 
stochastic system. In Program IV ( Fig. 5) , the simulation of discrete-time 
stochastic sysvems includes the generation of Gaussian random number. When 
we compare these simulated results with those of continuous-time equations 
(using Program lH modified Bunge-Kutta method) . it shows that this discrete- 
time model with At = 0.01 second is a good approximation of the continuous- 
time model. 


1020 DIM Y[51 
1030 T-0 
1040 H-100 

1050 I NPUT Y 1 1 1 ,Y [21 ,Y [31 ,Y [41 ,Y [51 

1100 y 


1110 

1120 

1130 

1140 

1150 

1160 

1165 

1170 

1175 

1180 


FOR 1=1 
B=RHD5 
H=fl+B 
NEKT I 
S=<R-6>: 
■I'C 1 ]=Y[ : 


TO 12 


0. 00000251 

1+H*V[ 2 ]+0. 5*Ht 2*Y[ 3 .1+250000* 


l-COSvO. 002*H> )*YC 4 ] 


T'2*s 


YC 1 ]=YC 1 j*-250000*(0.002*H-SIh<0.002*H>>*Y[5]+0.5*Ht-2 
Y[ 2 ]=YC 2 1+H*Y[ 3 1+500*3 1 tK O. 002*H>*V[ 4 1+50O*< 1-COS(0. 002*H> >*YC 5 1 
YC 2 ]=YC 2 3+H+S 
1 


■;[ 3 ]=YC 
U=YC 4 ] 

YC 4 ]=C0S < 0 . 002 *H ) * YC 4 1+8 1 N < 0 . 002* H > *YC 5 1 
YC 5 1=-S 1 0 . 002*H > +U+C0S ( 6 . 002+H > *Y C 5 1 
PR I HT T+H !. YC 1 1 > YC 2 1 » YC 3 ] » YC 4 1 » YC 
T=r+H 

1230 IF T<3001 THEN 1100 
1240 END 


1185 

1190 

1200 

1210 

1220 


53 


Figure 5. Program IV: discrete-time stochastic systems 



VI. SIMULATION OF ATTITUDE ESTIMATION BY KALMAN FILTER 


The previous four programs will be used in this section for the simulation 
of attitude estimation of Earth-orbiting satellites. The Space Telescope, which 
is an Earth orbiting satdlite, is used as an examfde in fois simulation and the 
Kh!man filter is used as the estimation tool. The discrete-time mathematical 
model of Space Telescope [1] can be represented by 


x(k + l) = x(k) + Bs(k) 


( 2 ) 


where s is a Gaussian random number with mean 0 and standard deviation 
2. 51 • 10~^ or covariance Q = 6. 28 * 10'*^ . The measuremoit equation is 
represented by 


y(k) = H x(k) + V 


( 3 ) 


where H=[l 0 0 0 0] is a row vector and v is the sensor noise which is 
also modeled as a Gaussian white noise with mean 0 and standard deviation 
7. 26 • 10"® or covariance R = 52. 7 • 10"* . 

Kalman filter for the estimation of state x(k) of the above system is 
^(k+1) = 0 x(k) + K(k+1) [y(k+l)-H^ x(k)) (4) 

and 

x(0) = HJfl 

where x(k 1) is the estimation of 1) represented as a com- 

bination of two terms: one is the previous estimate x(k) of the state and the 
other fo the correction Iqr the current measurement y(k + 1). K(k + 1) is 
called the optimal gain which is determined by minimizing mean square 
estimation error and can be calculated by 
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( 5 ) 


K(k+1) = P(k+ l|k) (H P(k+ l|k) + R)"‘ 


urhere P(k + 1| k) is called a priori error covariance matrix and is obtained 
from a posteriori error covariance matrix P(k|k) as 


P(k+l|k) = <l> P(k|k) + BQB^ 


( 6 ) 


and 


P(0|0) = Po 


Furthermore P(k|k) is obtained from the previous stage a priori error 
covariance matrix P(k|k-1) as follows: 

P(k|k) = (1 - K(k) H) P(klk-l) . (7) 

It is assumed that the random numbers s(k) and v(k) are uncorrelated. 
The initial state x(0) is unknown and is modeled as a Gaussian random variable 
with mean = 0 and covariance Po = 1 ( identity matrix) . 

The calculation procedures of Kalman filter program (Program V, Fig. 

6) are as follows: 

a. Calculate P(l|0) using initial value P(0|0) = Pj and equation (6). 

b. By P(1|0) and equation ( 5) to calculate K(l) (in general, this 
procedure involves a matrix inversion.) . 

c. Generate Gaussian random number s(k) and, for the simulation 
purpose, assume an initial value \{Q) then use equation (2) to calculate the 
state x( 1) . 

d. Generate random variable v(k), then use x(l) and equation (3) 
to obtain the simulated output y(l) . 
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1^ tM t- MLMf'f i f- i L r t F' i I HI ll h I 1 "I i 
DIM J'CieiJ«K:[‘:.].Nf5ii';.]<i:L:.."il.L:f .1 mM 1. 

REM !=i* • Y=H 

REM :: i>> EsrifiATL-: nr true s-rHit; 

I NR'JT KC <5 ] . : :t ;• ] - ;:c s ] » : :t . : :[ i n j 

REM IHITIftL VRLUES OF fc^UMRIt:. OTHTE HN 
I HPUT : ;c 1 ] 1 xc : ] . xc 5 ] » xc 4 ] . : ;c 5 ] 

INPUT p[l» 1 J.p[2.2]*p[ .•:.:,:],PC 4 ]-PCf »5] 

> FOR 1=1 TO '5-1) 

) FOR JsI+1 TO 5 


';.].FtS]»Gt5'5J*0t5: 


STRIE HND ERPOP i.OVFlPIPiNCE 


150 

FOR 

1=2 TO 5 

IbO 

FOR 

J=1 TO I-l 

170 

PC I* 

J]=PC Ml 

1:30 

NEXT 

J 

1 •?0 

NEXT 

I 

200 

T»0 


210 

H=0. 

01 

220 

N=5 


230 

Ct 1 • 

1 ]=1 

240 

CC 1 » 

2]=H 

250 

Ctl. 

3]=0.5*Hf.: 

260 

CC 1 . 

4 ]=25n0i:iM* •. l-Cii'I. ' •III.’ * 

270 

CC 1 . 

5 1=250000 - ' 0 . 002-H ] N ' 

200 

CC2i 

1 1=0 

290 

CC2. 

21=1 

300 

CC2. 

31=H 

310 

CC2» 

4 J=50y =3 1N‘:0. 0021*^1 ■ 

320 

CC2. 

5 J=50U'^ 1 -COS '1 0 . Om jrH • ■ 

330 

CC3» 

1 3=0 

340 

CC3» 

2 3=0 

350 

CC3. 

3 3=1 

360 

CC 3. 

4 3=0 

370 

CC3i 

5 3=0 

3S0 

CC4f 

1 3=0 

390 

CC4. 

2 3=0 

400 

CC4* 

3 3=0 

410 

CC4i 

4 3=COS'O.002>H' 

420 

CC4. 

5 3*SIN'. 0.002’-H ' 

430 

CC5* 

1 3=0 

440 

CC5. 

2 3=0 

450 

CC5. 

3 3=0 

460 

CC5« 

4 3=-MN* M.ri02^N ' 

470 

cr 5* 

1 s r 1 1 < 1*1 t u'i ♦ w 1 

480 8(1,11« 

0.6*Ht2 

400 8(3.1] • 

H 


0 . 01 . 1 ^: ' 


Figure 6. Program V: Kalmar, filter simulation. 
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500 B [3, 11=0 
510 B14,11=0 
520 B [5,1 1=0 

530 HCl5n=l 
540 HCl-2]-=0 
550 HC 1 !. 3 ]=0 
560 HCl-41-0 
570 H[1..5]=0 

575 REM CflLCULHTE H PRIORI Pd-J- 

580 FOR 1=1 TO N 

590 FOR J=1 TO N 

600 DC I > J ]=0 

610 NEXT J 

620 NEXT I 

630 FOR 1=1 TO H 

640 FOR L=1 TO N 

650 FOR K=1 TO N 

660 DC I , L ]=D[ I , L ]4 CC I , K ]*PC K , L 3 

670 ne:;t k 

672 NEXT L 

674 NEXT I 

680 FOR 1=1 TO N 

690 FOR J=1 TO N 

700 OC I , J 3=0 

710 NEXT J 

720 NEXT I 

730 FOR 1=1 TO N 

740 FOR J=1 TO N 

750 FOR L=1 TO N 

760 QC I > J 3=QC I J J 3+DC I < L ]*C[ J? L 3 

770 NEXT L 

780 PC 1 1 J 3=Q[ I ? J 3+0. 00000000000628*8'' I j 1 3*BC J> 1 3 
790 NEXT J 
800 NEXT I 

805 REM CflLCULRTE OPTIMRL KRLMRN GRIN 
810 FOR 1=1 TO N 

820 KC 1 3=PC 1 . I 3/-;; PC 1 , 1 3 +0. 0000527 > 

830 NEXT I 

832 PR I NT KC 1 3 . KC 2 3 5 KC 3 3 j KC 4 3 » KC 5 3 

834 PRINT PC 1 J 1 3.1 PC 1 ,i 2 3i PC 1 K3 3i PC 1 1 4 3i PC 1 ,i 5 3 

835 REM RRNDOM NO. GENERRTION 

836 PR I NT PC 3 1 1 3 !• PC 3 1 2 3 1 PL 3 < 3 3 1 PC 3 .-4 3.. PC 3 1 5 3 

837 PR.INT PC4- 1 3iP[4»23iPC4i335pC4,43jPC4i53 

838 PRINT PC5i 1 3 iPl5i23iPC5i33iPC5j43iPC5»53 
840 R1=0 

850 R2=0 

860 FOR I--1 TO 12 
870 B1=RND5 
880 B2=RND6 
890 A1=A1+B1 
900 A2=A2+B2 
910 NEXT I 

Figure 6 . ( C ontinued) . 


REPRODUClBILnY OF THK 
ORIGINAL PAGE IS POOR 








■r'WT’S* 


4^.--v.:-. . 4 - 


/ 


920 S»(A1-6)*0.00000251 
930 V=(A2-6) *0.00726 

935 REM CALCULATE TRUE STATE AND OUTPUT FOR SIMULATION 

940 FOR 1=1 TO 5 

950 EC I ]=0 

960 NEXT I 

970 FDR .1=1 TO 5 

980 FOR K=1 TO 5 

990 EC J 3=EC J ]+CC J? K ]*XC K+5 3 

1000 NEXT K 

1010 EC J3=EC J3+BC Jj 1 3*S 
1020 NEXT J 
1022 FOR 1=1 TO 5 
1024 XC I+5 3=E[ I 3 
1026 NEXT I 
1030 Y=XC6 3+V 

1035 REM CflLCULftTE ESTIMATED STATE 

1040 FOR 1=1 TO 5 

1050 FC 1 3=0 

1060 NEXT I 

1070 FOR 1=1 TO 5 

1080 FOR J=1 TO 5 

1090 FC 1 3=FC 1 3+CC I. J3^KCJ3 

1100 NEXT J 

1110 XCI3=FCI3+KCII*<Y-FL13) 

1120 NEXT I 

1130 REM CALCULATE ERROR COVARIANCE MATRIX , ■ 

1160 FOR 1=1 TO 5 

1170 FOR J=1 TO 5 • 

1180 GC I J J 3=PC I » J 3-KC I 3*PC 1 » J 3 . 

1185 GC Ij J3=GC I.J3-GC M 3*KC J 3+0. O000527*KC 1 3*KC J 3 

1190 NEXT J 

1200 NEXT I 

1202 FOR 1=1 TO 5 

1204 FOR J=1 TO 5 

1206 PC I» J3=GC I» J3 

1207 NEXT J 

1208 NEXT I 

1210 PRINT T+H> XC 1 3* XC 2 30<L 3 3’ XC 4 3? XC 5 3» XC 6 3» XC 7 3» XC 3 3» XC 9 3» XC 10 3 

1220 PRINT PC 1.- 1 3.-PC 1>23>PL 1>3 3,PC l!.4 3-FC 1».53 

1222 PRINT PC2»13>PC2»23>PC2>33>PC2»43»FC2»53 

1224 PRINT PC3i 1 3»PC3»23»PC3 j33>PC3>43jPC3»53 

1226 PRINT PC4» 1 3»PC4>23»PC4»33>PC4»43»FC4»53 

1228 PRINT PC5> 1 3.PC5»23.PC5»33.PC5.43»PC5»53 

1230 PRINT XC6 3-XC 13.XC7 3-XC2 3»XC6 3>XC3:»XC9 3-XC4 3»XC 10 3-XC5 3 ' 

1240 T=T+H 

1250 IF T<1 THEN 588 
1260 END 


Figures. (Concluded). 
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^e. By the output y(l) and optimal gain K(l) to obtain the estimated 
state x(l) from equation (4) with ^(0) - m^ . 

f. Next calculate P(l|l) by equation (7), 

g. Procedures a through f are repeated to find x(2) , x(3) , etc. 

Inputs of Program V are as fellows: 

a. Initial state of this system (five components): x(6), x(7), x(8), 
x(9) and x(l0). 

b. Initial values of estimated state: x(l)» x(2), x(3)» x(4) andx(5). 
These values are usually selected equal to the mean values of the system’s 
initial state. 

c. Initial values of diagonal elements of error covariance matrix Po . 

Outputs of Program V are as follows: 

a. K(l), K(2), K(3), K(4) and K(5) - optimal gains 

b. T+h, x(l), x(2), x(3), ...» x(l0) where 
T+h — sampling instant, 

x( l) , x(2) , .... x(5) — estimated states for the system, 
x(6), x(7), ...» x(l0) — simulated states of the system, 

c. P(I,J), I,J = 1, 2, 3, 4, 5 error covariance matrix, 

d. x(6) -x(l), x(7) -x(2), x(8) -x(3), x(9) - x(4) andx(lO) - 
x( 5) — estimated errors. 

A flow chart of this program is shown in Figure 7. 

A summary of the simulation results using the Kalman filter li. presented 
in the following paragraphs. 

With initial estimated state xj(0) = 0 , two computer rLi.., Hjve been 
executed in the simulation study for initial state xi(0) = 1 and 5 respectively. 

It is shown in Figure 8 that the estimated state Xi(k) approaches the true 
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Figure 8. Comparison between true state and estimated state using 
Kalman filter (with Xi(0) = 0), 


state xi(k) very fast and then follows the trajectory of the true state precisely# 
Figure 8 also shows that the accuracy (or characteristics) of the Kalman filter 
does not depend on the initial value of the system’s state» i.e., the simulation 
results are not sensitive with respect to initial state values. 
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Figure 9 presents the estimation errors and the rms error curve. The 
rms error curve is a root mean square error curve obtained from the diagonal 
elements of error covariance matrix P of Kalman filter. It gives a measure 
(in the probability sense) of the error at time K. That is» most of the error 
points in Figure 9 will lie within the two dashed lines (i.e. » rms error curves) . 
This curve can be obtained without using measurement information. In addition, 
the estimated errors shown in Figure 9 are very small and will decrease even 
more with time. 



Figure 10 presents the variation of rms error curves for different Q*s 
and R's which are the covariance matrices of disturbances and sensor noises, 
respectively. Four computer runs have been executed for this study: 
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From the curves of Figure 10, it is shown that for this system, the rms 
error curve is not sensitive with respect to Q values, but is sensitive with 
respect to different R's. To obtain a more accurate estimation requires smaller 
R values. (Note that all curves are obtained using Pn(0) = 10“^. ) 


VII. SIMUWTION OF AHITUDE ESTIMATION BY DECOMPOSED 
LINEAR RECURSIVE FILTER 


Decomposed linear recursive filters were developed previously [ 1 ) for 
the attitude estimation of Earth -orbiting satellites. These filters were shown 
in Reference 1 to have computational advantages over Kalman filter. The 
simulation procedures and results of attitude estimation of Space Telescope 
using decomposed linear recursive filter are presented in this section. The 
discrete-time mathematical model of Space Telescope ( 1) can be written as 


2 ^(k+l) - Ci 2 ^(k) + C 2 ^(k) + Bj s 


z(kFl) = C3z(k) 


( 8 ) 


where ^ and ^ arc the state variables of the overall system, i. e. , X” J 

s is a Gaussian white noise ns described in the previous section; and 


1 At 
0 1 


At sampling period. 


0..'; • (At)-' 2 r,i) 000 (I - cos 0,002 At) zrjo 000 (0.002 At - sin 0.002 At) 


At 


."iOO sin 0.002 At 500 (1 - cos 0,002 At) 


n. 


0 . 5 * (At)^ 
At 
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1 


0 


0 


Cs == I 0 cos 0. 002 At sin 0. 002 At 


0 -sin 0.002 At cos 0.002 At 


The measviTement equation is 


y(k) = H 


|s(k) 


+ V 


( 9 ) 


where row vector H and Gaussian white noise v are defined as those of the 
previous section. 

To estimate the state of this system, we use decomposed linear recursive 
filter which is described as follows: 


^(k) = x(k) + V^(k) V^'Vk) z(k) 

( 10 ) 

z(k+l) = Cji(k) + K (k+1) ly(k+l) - Hj C, ^(k) -HiC 2 z(k)l 

mmm 2 


where ^ and z are estimated states of and z, z(0) = [10"*, 4 • 10"*, O]"^, 
and 


a. x(k) is obtained from the following formula: 


x(k+l) = Cl x(k) + K^(k+1) ly(k+l) - Hi Cl x(k)I (11) 

where Hi - U 0), x(0) = (0 0)^, and K^(k+1) satisfy 
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T 


K^(k+ 1 ) = Pj^(k+l|k) H, (H, p (k+llk) h/ + R| 


p^(k+l|k) = Cip^(klk) c/ + B, QB,*^ 


p^(0|0) = 


b, V and V are obtained from 
X z 


V^(k+ 1 ) = (l-K^(k+l)Hi) Ujk+1) 


V (k+1) = U (k+1) 
z* z 


u (k+1) - C, V (k) + C2 V (k) 

A A Z 


U (k+ 1 ) C3V (k) 

£* i-t 


V (0) 
x' ' 


0 0 0 


10 0 


0 0 oj ’ v^(0) =010 


0 0 1 
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(16) 


c. K (k+1) is given by 

z 

K (k) = V (k) M(k+1) vj(k) R“» 
where M(k+l) sattsfies 


M(kU) = M(k) - M(k) 


’u ' 

X 

T 


Pjj(klk-l) 0 


.,T 




H 

H 


u 

z_ 



0 0 


+ H 


'u ' 

X 


’u ' 

X 

T 

_1 

’u ' 

X 


M(k) 


T 

H 

H 


U 


U 



U 

rj 

L J 


£a 



4 


T 

H + R 


M(k) 


with initial value 


M(0) = 


io-< 0 0 

0 10 "* 0 

0 0 10 - 


(17) 


The procedures for calculating the estimated states in this program 
(Program VI, Fig. 11) arc as follows; 

a. From equation (13) using initial value P^(0|0) we get P^(l|0) , 

b. Then equation (12) and p (l|0) we can calculate K (l), 

X X 

c. System’s state and output arc obtained by first generating two 
Gaussian random numbers s and v and then we have x(k) and y(k) from 
equations ( 8) and ( 9) . 
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1450 REM DECOMPOSED LINEAR RECURSIVE FILTER SIMULATION 
1460 REM X-C‘X+B*S , Y-H»X+V 

1470 REM Xdl X(S) ESTIMATES OF TRUE STATES X(6) XdOl 

If.dO DIM ::t 11 I-Ff ‘.m', ],ci s.‘j l,i:| '■» . ;.HI l l. ' ^ ].Ft j-ClT.-':- J.il :;.l ] 

151C1 DIN VC 5- ::]»LIC!.*3 J.ML J-lHC i J.kf - !: • J-IU 

1510 H=0.O1 

1530 CC 1» 1 3=1 

1540 CClfl]=H 

1550 CC l»33=0.5^ht3 

1560 CC1.4]»15 0O0A-^ 1, 1 -I, OS' I'll 1- II ■ ■ 

1570 CC 1 • 5 3=250000 » ■; 0 . 00.>l I ' • III*. O . i - H ■ • 

1500 CC2»13=0 
1530 CC2.2 3=1 
1C00 CC2*3 3=H 

1010 CC2»43=500'^3IM(0.0ul<H ' 

1 020 CC 2 > 5 3=500* 1 -COS ' 0 . OOl-^H > ■ 

1030 CC3 - 1 3=0 
1040 CC3>2 3=0 
1050 CC3.3 3=1 
1000 CC3«43=0 
1070 CC3>53=0 
1030 CC4,13=0 
1030 CC4i23=0 
1700 CC4. O=0 
1710 CC 4 > 4 J=COS • 0 . '-ly £- H ■ 

1720 CC4»5 3=SIN'.0.002*H> 

1730 CC5j 13=0 
1740 CIS. 2 3=0 
1750 CCS. 3 3=0 
1700 CC5.4 3= -i IH' 0.OO2*H ' 

1770 C C 5 . 5 3 = C 0 ■ 0 . 0 0 2 * H 

1780 BC 1. 1 3=0.5*Ht2 

1730 BC2. 1 3=H 

1300 BC3. 1 3=0 

1810 BC4. 1 3=0 

1820 BC5. 13=0 

1830 HC 1 . 1 3=1 

1840 HC 1.23=0 

1850 HC1.3 3=tj 

1800 HC 1.4 3=0 

1370 HC 1.53=0 

2010 I HPiJT :<L 6 j , ; :i 7 J • ; :i y ,i • : ■ < ] • ; :i 1 " i 
2014 T=0 
2020 N=2 
2030 11=3 

035 C'EM -11111.11 ,'MLIIC ul I I Jilt' ll . ihTF HUH l>Pi.'l- m iVHk I hHi. E hhipi:: 

^040 PC !• 1 J=1 
2050 PC 2. 1 1=M 
2000 PC 1 . 2 3 

2070 PI2.2I-1 
2060 XIHI‘0 
2000 XI12I-0 


Figure 11. Program VI; deeom|)osed linear recursive filter simulation. 
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2100 FOR 1-1 TO N-. A 
2110 FOR J-1 TOM 
2120 V[U]-0 
2130 NEXT .1 
2140 MEXT I 
2150 VC3>n=l 
2160 VC 4» 2 3=1 
2170 V[5>33=1 
2180 FOR 1=1 70 M 
2190 FOR J=1 TO n 
2200 MCI»J]=0 
2210 NEXT J 
2220 NEXT I 
2230 Mtl> 13=0.0001 
2240 MC2>23=e.O001 
2250 MC3i3 3s0.0001 
2260 XC 3 3=0. 0001 
2270 XC 4 3=0. 0004 
2280 XC5 3=0 
2290 XC13=0 
2300 XC2 3=0 
2310 FOR I=l TO N 
2320 FOR J=1 TO N 
2330 IiCI»J3=0 
2340 NEXT J 
2350 NEXT I 
2360 FOR 1=1 TO N 
2370 FOR L=1 TO N 
2380 FOR K=1 TO N 
2390 DC M3=DC I.L3+CC I»K3^PtK.L3 
2400 NEXT K 
2410 NEXT L 
2420 NEXT I 
2430 FOR 1=1 TO N 
2440 FOR J=1 TO N 
2450 QCI»J3=0 
2460 NEXT J 
.2470 NEXT I 
2480 FOR 1=1 TO N 
2490 FOR J=l TO N 
2500 FOR L=1 TO N 
2510 QC I* J3=Ql I» Jl+Dt Jai 

2520 NEXT L 

2530 PC i» J 3«C'C I » J 3+0. i:iu0MOMCiiu?MO6.:.>t;r 1 , 1 3+ec j» i ] 
2540 NEXT J 
2550 NEXT I 

2555 REM CflLCUlJiTF: OFTIMRL I OLHON COIN 

2S60 FOR 1-1 TO N 

2S70 Kill -P(1.n/(P|1, 1)^.0000627) 


Figure 11. (Continued). 
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2580 NEXT I 

2581 PRINT K[11,K(21 

2582 PRINT P[1,11,P(1,21 

2583 PRINT PC 1 PC 2. 2 ] 

2587 REM RANDijM Mu. GEHERhTIuN 
2590 Hl=0 
2600 R2=0 

2610 FOR I=l TO 12 
2620 B1=RHD5 
2630 B2*RHD6 
2640 Fll=ftl+Bl 
2650 R2=Fl2+B2 
2660 NEXT I 

2670 S= <R1 -6 >^0. 00000251 
2680 V=(R2-6> *0.00726 

2685 REM CRLCULRTE TRUE STRTE RND OUTPUT FOR SIMULRTIOM 

2690 FOR 1=1 TO 5 

2700 ECn=0 

2710 NEXT I 

2720 FOR J=1 TO 5 

2730 FOR K=1 TO 5 

2740 EC J ]=EC J 3+CC J, K ]*XC K + 5 ] 

2750 NEXT K 

2760 EC J]=EC J]+BC J. 1 ]*S 
2770 NEXT J 
2772 FOR 1=1 TO 5 
2774 XCI+5]=ECIJ 
2776 NEXT I 
2780 Y=XC6]*V 

2785 REM CRLCULRTE ESTIMRTF.n -ITRTE 

2790 FOR 1=1 TO N 

2800 FCn=0 

2810 NEXT i 

2320 FOR 1=1 TO M 

2830 FOR J=1 TO N 

2840 FC I ]=FC I l+CC I, J1*:;C J J 

2850 NEXT J 

2360 XC lO+I ]=FC I 3+KC I ]*• V-F L 1 

2870 NEXT I 

2380 FOR 1=1 TO N+M 

2890 FOR J=1 TO M 

2900 UCI»J]=0 

2910 NEXT J 

2920 NEXT I 

2930 FOR 1=1 TO N 

2940 FOR J=1 TO M 

2950 FOR K=1 TO N+M 

2960 UlUl-Ull,Jl+Cll,K)*VIK,J) 

2970 NEXT K 


Figure 1 1 , (Continued) . 
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2980 NEXTJ 
2990 NEXT I 

3000 FOR I=NH TO H+M ' 

3010 FOR .1=1 TO M 

3020 FOR K=N+1 TO H+M 

3030 UC I . J ]=UC I » J ]+C[ I , K 3*V[ K ? J 3 

3040 NEXT K 

3050 NEXT J 

3060 NEXT I 

3078 FOR I=l TO N 

3080 FOR J=1 TO M 

3090 VC I » J 3=IJC I » J 3-Kt 1 3*IJ[ 1 » J 3 

3100 NEXT J 

3110 NEXT I 

3120 FOR I=N+1 TO H+M 

3130 FOR J=1 TO M 

3140 VC I. J]=UCI» J3 

3150 NEXT J 

3160 NEXT I 

3170 P=0 

3180 FOR K=1 TO 3 

3190 F'=P+UC 1?K3^<MCK5 1 3*U[ 1? 1 3+nCK>23*U[ 1 > 2 3+MC Kj 3 3#UC 1 » 3 3> 

3200 NEXT K 

3210 FOR 1=1 TO 3 

3220 FOR J=1 TO 3 

3230 NCI>J3=0 

3240 OC I J 3=0 

3250 NEXT J 

3260 NEXT I 

3270 FOR 1=1 TO 3 

3280 FOR J=1 TO 3 

3290 FOR K=1 TO 3 

3300 OC I J J ]=0C I > J ]+MC 1 » 3+UC 1 » K 3+UC t ^ J 3 

3310 NEXT K 

3320 NEXT J 

3330 NEXT I 

3340 FOR 1=1 TO 3 

3350 FOR J=1 TO 3 

3360 FOR K=1 •^0 3 

3370 NC ! » J 3=HC I » J 3+OC I . K 3+MC K » J 3 

3380 NEXT K 

3390 NEXT t 

3400 NEXT I 

3410 FOR 1=1 TO 3 

3420 FOR J=1 TO 3 

3430 fit I » J 3=ri[ I » J 3-NC I. » J ]/ PC 1 » 1 3+P+0 . OC 00527 > 

3440 NEXT J 
3450 NEXT I 
3460 FOR I“1 TO " 


Figure 11. 


(Continued). 
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3470 FOR J=1 TO 5 
3480 NII,J1=0 
3490 NEXTJ 

3516 NEXT I 

3512 FOR 1=3 TO 5 

3514 K[ I ]=0 

3516 NEXT I 

3520 FOR I=N+1 Til N + M 

3530 FOR J=1 TO 3 

3540 FOR K=1 TO 3 

3550 NT I ? J ]=N[ I . J ]-i V[ i . K ]-M[ K ? J ] 

3560 NEXT K 

3570 KC I 3=K[ 1 J+NC I j J 3^V[ 1 j J 1 
3580 NEXT J 

3590 KC I ]=K[ I ]. 0„ 0000527 

3600 NEXT I 

3610 FOR 1=1 TO 2 

3620 FOR J=1 TO 2 

3630 GC I J J ]=P[ I J J ] -KC I ]*P[ 1 J J ] 

3640 NEXT J 

3650 NEXT I 

3660 FOR 1=1 TO 2 

3670 FOR J=1 TO 2 

3600 PC 1 1 J ]=GC I , J ] 

3690 NEXT J 

3700 NEXT I 

3710 FOR 1=1 TO 5 

3720 FOR J=1 TO 5 

3730 NCI* J]=0 

3740 GC I • J ]=0 

3750 NEXT J 

3760 NEXT I 

3770 FOR 1=1 TO 2 

37y0 FOR L=1 TO 2 

3790 FOR .1=1 TO 3 

3800 FOR K=1 TO 3 

38 1 0 GC I • J ]=G[ I ^ J 1+ VC I , K T*HC K ? J ] 

3820 NEXT K 

3830 Nl I » L ]=N[ I . U + GC I , J ]*VL L -J ] 
3840 NEXT J 

3850 NC I»L]=NC I,L.T+P[ I-l.] 

3860 NEXT L 
3870 NEXT 1 
3880 FOR 1=1 TO 2 
3890 FOR J=1 TO 3 
3900 FOR K=1 TO 3 

3910 NC I J J+3 ] -NC I - J + 2 ] + Q I . I T ^VC J+ 
3 9;:^fi NFXT y: 

3930 NEXTJ 
3940 NEXT I 


Figure 11. (Continued) 
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3950 FOR 1=1 TO 3 
3960 FOR J=1 TO 3 
3970 Gtl,Jl=0 

:S9S0 NEXT J 
39'?0 NEXT I 
4006 FOR 1=1 TO 3 

4010 FOR Jrl TO 3 

4020 FOR 1-=1 TO 3 

4030 FOR K=1 TO 3 

4040 Q[ I , L ]=0[ I > L ]■* V'[ I +2 » K ]»M[ K j L ] 

4050 NEXT K 

4060 NC I+2> J+2]=N[ l42.Jf2]+G[ I » L ]^VU+2 » L ] 

4062 G[Ii>L]=0 
4070 NEXT L 
4080 NEXT J 
4090 NEXT 1 

4100 PRINT N[1i1]..Nl1.2]»N[1.-33,.N[1j4].NC1!.5] 

4102 PRINT NC2> 1 3-N[2.2]jNC2-3]jNC2»4]jNC2j5] 

4104 PRINT N[3i>33.NC3,4]fH[:3.5] 

4106 PRINT N[4f 3 ]jN[4,4]»NC4.5] 

4108 PRINT NC5>3I»NC5.4].H[5.5] 

4610 FOR 1=5 TO 5 

4620 FCI]=0 

4630 NEXT I 

4640 FOR 1=3 TO 5 

4650 FOR .1=3 TO 5 

4660 F[ I ]=F[ I ]-»C:[ I^ J]*XtJ] 

4670 NEXT J 
4630 NEXT I 

4690 Y=Y-CC 1 • I ]*: :t 1 ]-C t 1 » 2 ]*XC 2 I-Ct 1 > 3 3i(XC 3 ]-CC 1 » 4 ]«XC 4 ]-CC 1 j 5 ]»XC 5 ] 
4700 FOR 1=3 TO 5 
4710 X[ I ]=FC I 3+KC I l*y 
4720 NEXT I 

4730 RC1» 1 ]=3QF;(VC3» 1 ];■ 

4740 FOR K. = 2 TO 3 

4750 RC1jK]=VC3.K]/R[1!.1] 

4760 NEXT K 
4770 J=2 
4780 S=0 

4790 FOR 1=1 TO J-1 
4800 8=S+R[ I . J ] t-2 
4810 NEXT I 

4820 R C J < J 3 = S 0 R •; V [ J + 2 • J 3 - S > 

4830 IF J=3 THEN 5020 
4840 FOR K=J+1 TO 3 
4850 U=0 

4860 FOR 1=1 TO J-1 
4870 U=U+Pr I - J 

4880 NEXT- I 

4890 R[J,K]«(V[J+2,K1-U)/R[J,J1 
4900 NEXT K 


Figure 11. (Continued). 
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5000 J=J+1 
5010 GOTO 4780 
5020 FOR J=1 TO 3 

5030 fi[J» J] = l . Kt J j 
5040 NEXT J 
5050 1=3-2 
5060 FOR K=IH 20 3 
5070 V=0 

50S0 FOR L=I+1 TO K 

5090 V=V+F:C I.L]*flI LjKJ 

5100 NEXT L 

5110 fl[ l!.K]=-V.RC 1. 1 J 

5120 NEXT K 

5130 1=1-1 

5140 IF i;.o THEN SObO 
5150 FOR 1=2 TO 3 
5160 FOR K=1 TO I-l 
5170 fl[I>K]=0 
5180 NEXT K 
5190 NEXT I 
5200 FOR J=1 TO 3 
5210 FOR L=1 TO 3 
5220 VCJ+2,L]=0 
5230 FOR K=1 TO 3 

5240 VC J + 2> L ]=Vt J+2> L ]fH[ l-K ]*0[ L - K J 
5250 NEXT K 
5260 NEXT L 
5270 NEXT J 

5280 PR I NT VC 3 , 2 J . '/C 3 , 2 J i VC 3 .< 3 J - VC 4 - 1 J ^ V C 4 

5300 FOR 1=1 TO 2 

5310 FOR J=1 TO 3 

5320 NCI» J]=0 

5330 NEXT J 

5340 NEXT I 

5350 FOR 1=1 TO 2 

5360 FOR K = 1 TO 

5370 FOR J=1 TO 3 

5380 NC I > K ]=N( I , K 3+VC I • J 2 * VL J f2 - k ] 

5390 NEXT J 


VC 4 1 3 1 !. Vt 5 J 1 ] » VC 5 » 2 3 » VC 5,33 


5400 NEXT K 

5410 xc 1 3=;:c If 10 3+n: i . i j-mcxu j*ni. i > :: i*:- 1 : 2 > 2 .h-mc im^xt 2 + 3 3 

5420 NEXT I 

5430 PRINT ' fH»XC 1 3.XC2 l-XC 3 3*Xt 4 XC 5 3,XC6 MCf 7JOaSJ»Xt 9 3. XC 10 3 
5440 PRINT XC 6 3-XC 1 3? XI 7 J -Xi 2 i« ,<l 2 J - XC 3 ■ - IT 9 3-XC 43. XC 10 3-XC 5 3 


5450 T=T+H 


5460 IF T<0.1 Tt)EN ?21u 
5470 END 


i 


Figure 11, (Concluded). 
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d. Usir^ K and y(k), equation (11) gives x(k). This vector will be 
used to construct 3^ . 

e. Calculate V (k) and V (k) by equations (14) and (15), M(k+1) 

X z 

is obtained by equation (17). 

f. Optimal gain K (k) can be obtained from equation (16) , 

z 

g. Finally 5 (k) and z(k) are calculated by equation (10). 

h. Before the second Iteration is executed, we need to find p (klk) 

using 

p^(k| k) = (I - K^(k) Hi) p^(k| k-1) 

i. Then the procedures a to h are repeated. 

j. To have more insist concerning the simulation resvilts, we need to 
calculate the error covariance matrices which can be found by 

p^(klk) = p^(klk) + V^(k)M(k+l) vj(k) 

p^(k|k) = V^(k) M(k+1) vj(k) 

Inputs of this program ( Progran. , .-e 

x(6), x(7), x(8), x(9) a’ 0) — initial values of system’s state. 
Outputs of Program VI are 

a. K( l) , K(2) — optimal gains of Kalman filter for the bias-free 
subsystem. 


3 (> 


b. P( 1, 1) , P( 1, 2) , P( 2, 1) • P( 2, 2) — a priori error covariance matrix 
of the bias -free subsystem. 

c. V (K) — a 3 X 3 matrix (this printout is used to check the results 

of V "*(k) £&own later), 
z 

d. M(k) — a 3 X 3 matrix used to construct the error covariance matrix. 

e. N(Iy J) — a 5 X 5 matrix which is a posteriori error covariance 
matrix (common symbol is P(k| k) ] . 

f. V(I,J) — a 3x 3 matrix which is V ”* used to construct estimation 

of ^(k). * 

g. T+h, x(l)» x(2)y ...y x(lO) \^ere 
T+h — sampling instanty 

x(l)y x(2)y ...y x( 5) — estimsted statOy 
x(6)y x(7)y ...y x( 10) — stato of thc system. 

h. x(6) -x(l)y x(7) -x(2)y ...y x(10) -x(5) — estimation errors. 

Figure 12 presents a flow chart of this program. 

Simulation results using decomposed linear recursive filter are 
summarized in the following paragraphs. 

Figure 13 presents the true state Xi(k) and the estimated state ^(k) 
using decomposed linear recursive ^ter for two cases (x|(0) = 1 and 5 respec- 
tively) . The results show that this filter is accurate in estimation and fast in 
response. It is interesting to point out that these simulation results and those 
of using Kalman filter (Fig. 8) match very well. But for higher order systems y 
this filter will give a better estimation than that of Kalman filter. Decomposed 
linear recursive filter has computational advantages, i.e . , less Integration 
errors and roimdoff errors. 

Figure 14 gives the results of the state X 2 (k) and the estimated state 
X 2 (k) X 2 (k) is the time rate of pitch ang^e. 
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TRUE STATE 
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TRUE STATE x.{k) AND ESTIMATED STATE 






ERROR x,<k) - x,(k) 


Figures 15 and 16 give the estimation errors X{(k) - ^(k) and rms 
error curves. Figure 15 presents the case with xj(0) = 1 and 5^(0) = 0, 
Figure 16 presents the case with X|(0) = 5 and Sti(O) = 0 . It is also shown 
that this filter is not sensitive to the difierent values of the system’s initial 
state. As expected most of these error points in Figures 15 and 16 are within 
the area betwemi the two rms error curves. Furthermore, if we compare 
Figure 15 with Figure 9 the estimation error points match very well. 



Figure 15. Estimation error Xj(k) - J^(k) (with Xi(0) = 1, 

xi(0) = 0). 
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TIME t - 0.01 ■ K 


Figure IG. Estimation error Xi(k) -Xi(k) (with Xi(0) = 5, Xj(0) = 0). 


Figures 17 and 18 present the results for X 2 (k) - X 2 (k). The rms error 
curve for this case also converges with time; however, the convergence rate is 
not us fast as that of Figures 15 and 16. To observe the variation of this rms 
error curve for longer time interval, another computer run has been executed 
for 100 iterations and the results are shown in Figure 19. It shows that for 
state X 2 (k) , the rms error curve also converges at a satisfactory rate. 
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ERROR X2(k) - X2(k) 


I 

f / 
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TIME t - 0.01 • K 


Figure 17. Estimation error X 2 (k) - (with X2(0) = 1, ^^ 2 ( 0 ) = 0), 
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ERROR 



ERROR KjCk) - xjik) 



VIII. CONCLUSIONS 


There are six BASIC programs presented in this paper. The first four 
programs ( Programs I throu^ IV) were developed for the construction of simu- 
lation programs of the attitude estimation problem. The main results of this 
research are Programs V and VI — simulation of attitude estimation of Space 
Telescope by statistical filters (including decomposed linear recursive filter and 
Kalman filter) . Figures 8 through 10 and 13 through 19 show the simulation 
results. The advantages of the decomposed linear recursive filter are its 
accuracy in estimation and its speed in response time. Similar results were 
obtained by using Kalman filter; however, for iiigher order systems, the 
decomposed linear recursive filter has computational advantages (i.e. , fewer 
integration errors and roxmdoff ervors) over the Kalman filter. 
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